#include "modules/CtrlModule/PassivePlanner/SmallObsPlanner.h"
#include "lib/WrapFuncs.h"

namespace behavior_controller
{       
        #define TURN_REEOR 0.08726646 
        SmallObsPlanner small_obs_Planner;
        void SmallObsPlanner::run(double &linear_vel, double &angular_vel)
        { 
                if (!init_angle)
                {
                   std::cout<< "SmallObsPlanner： init_angle!"<< std::endl;
                   init_angle=true;
                   PoseF robot_pose_(0.0f, 0.0f, .0f),end_point(small_obs_Planner.mean_pose_local.x(), small_obs_Planner.mean_pose_local.y(), .0f);
                   double local_angle= std::atan2(end_point.y()-robot_pose_.y(),end_point.x()-robot_pose_.x());
                   global_goal_angle= Funcs::NormalizeAngle(DataSet::robot_pose.angle()+local_angle+ M_PI_2); // + 90deg
                }
                if(!finished_turn)
                {
                   double dst_angle= Funcs::NormalizeAngle(DataSet::robot_pose.angle() - global_goal_angle);
                   std::cout<< "TurnToAngle  dst_angle= "<< dst_angle  << "  global_goal_angle= "<< global_goal_angle << "  robot_pose.angle= "<< DataSet::robot_pose.angle()  <<std::endl;  
                   if(fabs(dst_angle) >= TURN_REEOR)  // 0.052359876rad ~= 3.0deg
                   {
                        TurnToAngle(linear_vel,angular_vel,dst_angle);
                        return;
                   }
                   else
                   {
                        finished_turn=true;
                        turn_sum=0;
                        total_angle= 2*M_PI;
                        global_goal_angle= global_goal_angle + 0;
                        last_angle=DataSet::robot_pose.angle();
                        last_pose= DataSet::robot_pose;
                        linear_vel=0.15;
                        aim_dist= obs_dis;
                        angular_vel_aim=-linear_vel/turn_radius;
                        last_dis_p=-angular_vel_aim;
                   }
                }
                if(finished_turn)
                {
                        turn_sum += fabs(Funcs::NormalizeAngle(last_angle- DataSet::robot_pose.angle()));
                        last_angle=DataSet::robot_pose.angle();
                        double dis_sq=last_pose.DistanceSquare(DataSet::robot_pose);
                        //std::cout<< "Around : total_angle= "<< total_angle << "  turn_sum= "<< turn_sum << "  global_goal_angle= "<< global_goal_angle << 
                        //"  robot_pose.angle= "<< DataSet::robot_pose.angle() << " dis_sq= "<<dis_sq <<" turn_radius="<<turn_radius <<std::endl; 

                        if( !((turn_sum- total_angle)> 0 && dis_sq< 0.01 // error= 0.1m
                                && fabs(DataSet::robot_pose.angle()-global_goal_angle) <= 0.08726646) )//  0.08726646rad = 5deg
                        {
                                pid_adjustment(linear_vel,angular_vel); 
                        }
                        else
                        {
                                finished = true;
                                finished_turn= false;
                                init_angle= false;
                                linear_vel=
                                angular_vel=0;
                                small_obs[small_obs.size()-1].first=true;
                        }
                }
        }


        void SmallObsPlanner::TurnToAngle(double &linear_vel, double &angular_vel,double &dst_angle)
        {
                if (dst_angle > 0.17 || dst_angle < -0.17) //当角度差相差0.1弧度时    1deg=0.017453292rad
                {
                        linear_vel = 0;
                        angular_vel = -dst_angle;
                }
                 else if (dst_angle > TURN_REEOR &&  dst_angle < 0.17) //当角度差小于0.1弧度时,大于0.03弧度时 
                {
                        linear_vel = 0;
                        angular_vel = -0.12;
                }
                else if (dst_angle > -0.17 && dst_angle < -TURN_REEOR) //当角度差小于0.1弧度时,大于0.052359876弧度(~3度)时
                {
                        linear_vel = 0;
                        angular_vel = 0.12;
                }
                else
                {
                        linear_vel = 0;
                        angular_vel = 0;
                }
        }
         void SmallObsPlanner::pid_adjustment(double &linear_vel, double &angular_vel)
        {
                linear_vel = 0.15;
                angular_vel=angular_vel_aim;
                /*if(point_count> 0 && obs_dis>0)
                {
                   double dis_p = obs_dis - aim_dist;       //当dis_p>0时，应向右转
                   double dis_d = dis_p - last_dis_p;        //当dis_d>0时，说明变化趋势在增大，应反作用于角速度
                   angular_vel = -dis_p * 25 - dis_d * 1.2; //角速度输出，需要调节p d参数，使机器人行走轨迹不至于过于晃动。
                   //angular_vel = angular_vel> 0.15 ? 0.15:angular_vel;
                   //angular_vel = angular_vel< -0.15 ? -0.15:angular_vel;
                   //angular_vel=angular_vel+ angular_vel_aim;
                   last_dis_p = dis_p;
                   //std::cout<< "Around : dis_p= "<< dis_p <<  " obs_dis= "<< obs_dis <<  " aim_dist= "<< aim_dist << std::endl;
                }
                else
                {
                   angular_vel=angular_vel_aim;
                }*/
        }
}